/** \file jtag.c
 *
 * Low level routines for JTAG access.
 * Most of these functions use the JTAG tools library written by
 * Marcel Telka, some functions were copied from his code to make linking
 * simpler.
 *
 * (c) 11/2004 Martin Strubel <hackfin@section5.ch>
 *
 * $Id: jtag.c 75 2005-11-11 09:12:34Z jiez $
 *
 *
 */


#include "tap.h"
#include "register.h"
#include <stdlib.h>
#include <string.h>

#include "emu.h"

////////////////////////////////////////////////////////////////////////////
// GLOBALS
//

// JTAG state sequences: first byte: number of cycles, 2nd: bit sequence
// asserted to TMS
//

// in state RUN_TEST_IDLE
// FIXME: clean up some of these, once they are no longer needed.

const
char s_jtag_shift_ir[]          = { 4, 0x03 };

const
char s_jtag_shift_dr[]          = { 3, 0x01 };

// in state UPDATE_DR, UPDATE_IR, TEST_LOGIC_RESET

const
char s_jtag_run_test_idle[]     = { 1, 0x00 };

// Any state:

const
char s_jtag_test_logic_reset[]  = { 5, 0x1f }; 


chain_t *g_chain;

#ifdef ORIG
static
char * const cable_params[] = {
	"ppdev",
	"/dev/parport0",
	"DLC5"
};
#else
char * const cable_params[] = {
	"parallel",
	"0x378",
	"WIGGLER"
};
#endif


////////////////////////////////////////////////////////////////////////////
// AUXILIARIES
//

// TODO: We have yet no proper error handling inside this jtag
// library.
//
int handleError(char *prefix, int line,  int error)
{
	char *s = "TODO: Error handler!!";

	printf("%s:%d Error: %s\n", prefix, line, s);
	return error;
}

////////////////////////////////////////////////////////////////////////////
//
// JTAG tools ripoff
//
// Quite some of this code was taken from the JTAG tools and slightly
// adapted...
//

static
int parport_init(chain_t *chain, char * const *parms)
{
	int i;
	/* search connection type driver */
	for (i = 0; parport_drivers[i]; i++) {
		if (strcmp(parms[0], parport_drivers[i]->type) == 0)
			break;
	}
	
	if (!parport_drivers[i]) {
		fprintf(stderr, "Unknown connection type: %s\n", parms[0] );
		return 0;
	}

	// XXX Ugly: uses (const char **) instead of (char * const *)
	chain->cable = parport_drivers[i]->connect((const char **) &parms[1], 2);

	if (!chain->cable) {
		fprintf(stderr, "Could not find desired cable connection\n");
		return 0;
	}

	if (cable_init(chain->cable)) {
		fprintf(stderr, "Could not init cable connection\n");
		return 0;
	}
	chain_set_trst(chain, 0);
	five_tclk_wait(); // Added PSW
	chain_set_trst(chain, 1);
	tap_reset( chain );

	return 1;
}

////////////////////////////////////////////////////////////////////////////

#define	DETECT_PATTERN_SIZE	8
#define	MAX_REGISTER_LENGTH	1024
#define	TEST_COUNT		1
#define	TEST_THRESHOLD		100		/* in % */


int
detect_register_size( chain_t *chain )
{
	int len;
	tap_register *rz;
	tap_register *rout;
	tap_register *rpat;

	for (len = 1; len <= MAX_REGISTER_LENGTH; len++) {
		int p;
		int ok = 0;

		rz = register_fill(register_alloc(len), 0 );
		rout = register_alloc( DETECT_PATTERN_SIZE + len );
		rpat = register_inc( register_fill( register_alloc(DETECT_PATTERN_SIZE + len), 0 ) );

		for (p = 1; p < (1 << DETECT_PATTERN_SIZE); p++) {
			int i;
			const char *s;
			ok = 0;

			s = register_get_string( rpat );
			while (*s)
				s++;

			for (i = 0; i < TEST_COUNT; i++) {
				tap_shift_register(chain, rz, NULL, 0);
				tap_shift_register(chain, rpat, rout, 0);

				register_shift_right(rout, len);

				if (register_compare(rpat, rout) == 0)
					ok++;
			}
			if (100 * ok / TEST_COUNT < TEST_THRESHOLD) {
				ok = 0;
				break;
			}

			register_inc(rpat);
		}

		register_free(rz);
		register_free(rout);
		register_free(rpat);

		if (ok)
			return len;
	}

	return -1;
}

static
parts_t *
detect_parts(chain_t *chain, char *db_path)
{
	int irlen;
	tap_register *ir;
	int chlen;
	tap_register *one;
	tap_register *ones;
	tap_register *br;
	tap_register *id;
	parts_t *ps;
	int i;

	/* Detect IR length */
	tap_reset(chain);
	tap_capture_ir(chain);
	irlen = detect_register_size(chain);
	if (irlen < 1)
		return NULL;

	printf("IR length: %d\n", irlen );

	/* Allocate IR */
	ir = register_fill(register_alloc(irlen), 1);
	if (ir == NULL) {
		printf("out of memory\n");
		return NULL;
	}

	tap_shift_register(chain, ir, NULL, 1);
	register_free(ir);

	/* Detect chain length */
	tap_capture_dr(chain);
	chlen = detect_register_size(chain);
	if (chlen < 1) {
		printf ("Unable to detect JTAG chain length\n") ;
		return NULL;
	}
	printf("Chain length: %d\n", chlen);

	/* Allocate registers and parts */
	one = register_fill(register_alloc(1), 1);
	ones = register_fill(register_alloc(31), 1);
	br = register_alloc(1);
	id = register_alloc(32);
	ps = parts_alloc();
	if (!one || !ones || !br || !id || !ps) {
		printf( "out of memory\n" );

		register_free(one);
		register_free(ones);
		register_free(br);
		register_free(id);
		parts_free(ps);
		return NULL;
	}
	chain->parts = ps;
	chain->active_part = 0;

	/* Detect parts */
	tap_reset(chain);
	tap_capture_dr(chain);

	for (i = 0; i < chlen; i++) {
		part_t *part;
		tap_register *did = br;		/* detected id (length is 1 or 32) */

		tap_shift_register(chain, one, br, 0);
		if (register_compare(one, br) == 0) {
			/* part with id */
			tap_shift_register(chain, ones, id, 0);
			register_shift_left(id, 1);
			id->data[0] = 1;
			did = id;
		}

		printf("Device Id: %lx\n", strtol(register_get_string(did), 0, 2));
		part = part_alloc(did);
		if (part == NULL) {
			printf("Out of memory\n");
			break;
		}
		parts_add_part(ps, part);

	}

	for (i = 0; i < 32; i++) {
		tap_shift_register(chain, one, br, 0);
		if (register_compare(one, br ) != 0) {
			printf("Error: Unable to detect JTAG chain end!\n");
			break;
		}
	}
	tap_shift_register(chain, one, NULL, 1);

	register_free(one);
	register_free(ones);
	register_free(br);
	register_free(id);

	return ps;
}


#ifdef DEBUG
#define CHAIN_CLOCK(chain, tms, tdi) chain_clock_log(chain, tms, tdi)
#else
#define CHAIN_CLOCK chain_clock
#endif

#ifdef DEBUG

BFIN_REGISTER g_ins;
int g_is_emuir = 0;

void chain_clock_log(chain_t *chain, int tms, int tdi )
{
	chain_clock(chain, tms, tdi);
	if (tap_state(chain) == Run_Test_Idle) {
		printf("Execute %08lx\n", g_ins);
	}
}
#endif

/** Go to JTAG state according to state string
 */

int jtag_goto_state(chain_t *chain, const char *state)
{
	int n = state[0];
	int seq = state[1];

	while (n--) {
		CHAIN_CLOCK(chain, seq & 1, 0); // Toggle through JTAG states
		seq >>= 1;
	}
	return 0;
}

/** Open JTAG port
 * 
 * \return 1: success, failure else.
 */

int jtag_open(char * const *argv, chain_t **controller)
{
	chain_t *chain;

	chain = chain_alloc();
	if (!chain) return 0;
	
	if (argv == 0) {
		argv = cable_params;
	}
	if (!parport_init(chain, argv)) return 0;

	detect_parts(chain, JTAG_DATA_DIR);
	printf("Active: %d\n", chain->active_part);
	// TODO: select part in JTAG chain

	*controller = chain;
	return 1;
}

/** Reset JTAG state machine. This is a soft reset, not all parports
 * have a connection to /TRST...
 * 
 */

int jtag_reset(chain_t *chain, int cycles)
{
	while (cycles--) {
		CHAIN_CLOCK(chain, 1, 0); // Keep TMS high
	}
	return 0;
}

/** Close JTAG port
 */

void jtag_close(chain_t *c)
{
	chain_free(c);
}


int jtag_queue(chain_t *c, int queue)
{
	return 0;
}

int jtag_flush(chain_t *c)
{
	return 0;
}

////////////////////////////////////////////////////////////////////////////
//


void jtagchain_select(chain_t *chain)
{
	g_chain = chain;
}

////////////////////////////////////////////////////////////////////////////
// These are the jtag wrapper routines per CPU
//
// These routines allow some careful control over JTAG state
// sequencing (which is very important on the BF.
//
// Note that whenever RTI state is entered, the content of the
// EMUIR is execute when in emulation mode.
//

// endian handler change this to 1 for BE host
#include <asm/byteorder.h>
#ifdef __LITTLE_ENDIAN
static int g_host_is_be = 0;
#else
static int g_host_is_be = 1;
#endif

static inline REGISTER swap32(REGISTER reg)
{

    if (g_host_is_be) {
	__swab32(reg);
    }
    return reg;
}

static inline REGISTER swap16(REGISTER reg)
{

    if (g_host_is_be) {
	__swab16(reg);

    }
    return reg;
}


static inline REGISTER le32toh(REGISTER reg)
{
    return swap32(reg);
}
static inline REGISTER htole32(REGISTER reg)
{
    return swap32(reg);
}


/** Shift register without entering Run-Test/Idle
 */

static
BFIN_REGISTER tapreg_shift_noRTI(unsigned long value, unsigned int width)
{
	int i, j, k;
	BFIN_REGISTER r = 0;

#ifdef DEBUG
	if (g_is_emuir) {
		g_ins = value;
	}
#endif

	width--;

	if ((tap_state(g_chain) != Shift_DR)) {
		printf("Invalid state: $%02x\n", tap_state(g_chain));
		printf("should be: %02x\n", Shift_DR);
		return 0;
	}

	for (i = 0; i < g_chain->parts->len; i++) {
		k = width;
		for (j = 0; j <= width; j++) {
			r |= (cable_get_tdo(g_chain->cable) << k);
			CHAIN_CLOCK(g_chain, (j == width) ? 1: 0,
					(value >> k) & 1);
			k--;
		}
	}
	CHAIN_CLOCK(g_chain, 1, 0);      // Update-DR
	return r;
}


int scanchain_select(int schain)
{
	chain_t *chain = g_chain;
	parts_t *ps;
	int i, j;
	int width = 5 - 1;

#ifdef DEBUG
	char *s;
	g_is_emuir = 0;
	switch (schain) {
		case SCAN_DBGCTL:
			s = "DBGCTL"; break;
		case SCAN_EMUIR:
			s = "EMUIR";
			g_is_emuir = 1; break;
		case SCAN_DBGSTAT:
			s = "DBGSTAT"; break;
		case SCAN_EMUDAT:
			s = "EMUDAT"; break;
		default:
			s = "<unknown>"; break;
			break;
	}
	printf("SCANCHAIN_SELECT %s\n", s);

#endif

	ps = chain->parts;

	tap_capture_ir(chain);

	CHAIN_CLOCK(chain, 0, 0); // Shift-IR
	for (i = 0; i < ps->len; i++) {
		for (j = 0; j <= width; j++) {
			CHAIN_CLOCK(chain, (j == width) ? 1: 0,
					(schain >> j) & 1);
		}
	}
	CHAIN_CLOCK(chain, 1, 0); // Update-IR
	return 0;
}



int scanchain_rti()
{
	CHAIN_CLOCK(g_chain, 0, 0);
	return 0;
}

int scanchain_shiftout(REGISTER *out, int exec)
{
        REGISTER hout;

	jtag_goto_state(g_chain, s_jtag_shift_dr);
	hout = tapreg_shift_noRTI(0, BFIN_REGISTER_WIDTH);
	*out=le32toh(hout);
	if (exec == EXEC)
		CHAIN_CLOCK(g_chain, 0, 0); // Run-Test/Idle
	return 0;
}

int scanchain_shiftin(BFIN_REGISTER r, int exec)
{
        REGISTER h;
	h=htole32(r);
	jtag_goto_state(g_chain, s_jtag_shift_dr);
	tapreg_shift_noRTI(h, BFIN_REGISTER_WIDTH);
	if (exec == EXEC)
		CHAIN_CLOCK(g_chain, 0, 0); // Run-Test/Idle
	return 0;
}

void scanchain_shift_twoins(BFIN_REGISTER ins0, BFIN_REGISTER ins1, int exec)
{
        BFIN_REGISTER h;

	jtag_goto_state(g_chain, s_jtag_shift_dr);

	h = htole32(ins1);
	tapreg_shift_noRTI(h, BFIN_REGISTER_WIDTH);
	jtag_goto_state(g_chain, s_jtag_shift_dr);

	h = htole32(ins0);
	tapreg_shift_noRTI(h, BFIN_REGISTER_WIDTH);
	if (exec == EXEC)
		CHAIN_CLOCK(g_chain, 0, 0); // Run-Test/Idle
}

int shift_dr16_out_msb(chain_t *chain, BFIN_REGISTER *out, int exec)
{
	BFIN_REGISTER r;

	r = tapreg_shift_noRTI(0, 16);
	if (exec == EXEC) {
		CHAIN_CLOCK(g_chain, 0, 0); // Run-Test/Idle
	}

	*out = le32toh(r);
	return 0;
}

int shift_dr16_in_msb(chain_t *chain, BFIN_REGISTER in, int exec)
{
	BFIN_REGISTER r;
	r = htole32(in);
	tapreg_shift_noRTI(r, 16);
	if (exec == EXEC) {
		CHAIN_CLOCK(g_chain, 0, 0); // Run-Test/Idle
	}

	return 0;
}

//PSW  from emulation.c

int jtag_getid(Cpu *cpu,unsigned long * id)
{
       int error;

       error = get_idcode(cpu, id);
       return error;
 }

